

#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#include "platform.h"

#include "build/build_config.h"
#include "build/debug.h"

#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"

#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"

#include "drivers/rx_nrf24l01.h"
#include "drivers/pwm_output.h"
#include "drivers/time.h"

#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"

#include "sensors/acceleration.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"

#include "rx/rx.h"

PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);

#ifndef TARGET_DEFAULT_MIXER
#define TARGET_DEFAULT_MIXER    MIXER_QUADX_1234
#endif
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
    .yaw_motors_reversed = true,
);

PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);

void pgResetFn_motorConfig(motorConfig_t *motorConfig)
{
    motorConfig->minthrottle = 1000;
    motorConfig->maxthrottle = 1600;//TODO 最大油门

    motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
    motorConfig->dev.motorPwmProtocol = PWM_TYPE_STANDARD;
    motorConfig->mincommand = 1000;
    motorConfig->digitalIdleOffsetValue = 450;
}

PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);

#define PWM_RANGE_MID 1500

#define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight

static float motorMixRange;

float motor[MAX_SUPPORTED_MOTORS];
float motor_disarmed[MAX_SUPPORTED_MOTORS];

float pidSumLimit;
float pidSumLimitYaw;

// #ifdef OLD_FRAME
// //   1
// // 4   2
// //   3
// const motorMixer_t currentMixer[] = {//mixerQuadP
//     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
//     { 1.0f, -1.0f,  0.0f,  1.0f },          // LEFT
//     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
//     { 1.0f,  1.0f,  0.0f,  1.0f },          // RIGHT
// };
// #else
// 1 2
// 3 4
const motorMixer_t currentMixer[] = {//mixerQuadX1234
    { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
    { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
    { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
    { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
};
//#endif

float motorOutputHigh, motorOutputLow;

static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
static uint16_t rcCommand3dDeadBandLow;
static uint16_t rcCommand3dDeadBandHigh;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;

float getMotorMixRange(void)
{
    return motorMixRange;
}

bool areMotorsRunning(void)
{
    bool motorsRunning = false;
    if (ARMING_FLAG(ARMED)) {
        motorsRunning = true;
    } else {
        for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
            if (motor_disarmed[i] != disarmMotorOutput) {
                motorsRunning = true;

                break;
            }
        }
    }

    return motorsRunning;
}

bool mixerIsOutputSaturated(int axis, float errorRate)
{
    UNUSED(axis);
    UNUSED(errorRate);
    return motorMixRange >= 1.0f;
}

// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void)
{
    disarmMotorOutput = motorConfig()->mincommand;
    motorOutputLow = motorConfig()->minthrottle;
    motorOutputHigh = motorConfig()->maxthrottle;
    deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
    deadbandMotor3dLow = flight3DConfig()->deadband3d_low;

    rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck;

    rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
    rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;

    rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
}

void mixerInit()
{
    initEscEndpoints();
}

void pidInitMixer(const struct pidProfile_s *pidProfile)
{
    pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit);
    pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw);
}

void mixerConfigureOutput(void)
{
    mixerResetDisarmedMotors();
}

void mixerResetDisarmedMotors(void)
{
    // set disarmed motor values
    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
        motor_disarmed[i] = disarmMotorOutput;
    }
}

void writeMotors(void)
{
    if (pwmAreMotorsEnabled()) {
        for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
            pwmWriteMotor(i, motor[i]);
        }
    }
}

static void writeAllMotors(int16_t mc)
{
    // Sends commands to all motors
    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
        motor[i] = mc;
    }

    writeMotors();
}

void stopMotors(void)
{
    writeAllMotors(disarmMotorOutput);

    delay(50); // give the timers and ESCs a chance to react.
}

static float throttle = 0;
static float motorOutputMin;
static float motorRangeMin;
static float motorRangeMax;
static float motorOutputRange;

void calculateThrottleAndCurrentMotorEndpoints(void)
{
    float currentThrottleInputRange = 0;

    throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
    currentThrottleInputRange = rcCommandThrottleRange;
    motorRangeMin = motorOutputLow;
    motorRangeMax = motorOutputHigh;
    motorOutputMin = motorOutputLow;
    motorOutputRange = motorOutputHigh - motorOutputLow;

    throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);

    //TODO 重力负反馈
    float temp = acc.accSmooth[Z] - GZero;
    temp = -temp * 0.00025f;
    float range = 0.1f;

    constrain(temp,range,-range);

    throttle += temp;

}

static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
{
    // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
    // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
        float motorOutput = motorOutputMin + (motorOutputRange * (motorMix[i] + throttle * currentMixer[i].throttle));

        if (failsafeIsActive()) {
            motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
        } else {
            motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
        }

        // Motor stop handling
        if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
            if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
                motorOutput = disarmMotorOutput;
            }
        }
        motor[i] = motorOutput;
    }

#ifdef USE_RX_NRF24
#ifndef IS_FIXED_WIN
    uint32_t temp = nrf24l01_x;
    temp = temp * 1000;
    temp = temp >> 8;
    temp = 1000 + temp + 300;
    //debuginfoFastInt[12] = temp;
    pwmWriteServo(0, temp);
#endif
#endif

#ifdef IS_FIXED_WIN

#ifdef FIXED_WIN_ROLL_REVERSE
        motor[0] = 1500 - 1000 * scaledAxisPidRoll;//rcData[ROLL];
#else
        motor[0] = 1500 + 1000 * scaledAxisPidRoll;//rcData[ROLL];
#endif

#ifdef FIXED_WIN_PITCH_REVERSE
        motor[1] = 1500 - 1000 * scaledAxisPidPitch;//rcData[PITCH];
#else
        motor[1] = 1500 + 1000 * scaledAxisPidPitch;//rcData[PITCH];
#endif
        
        if(manual){
#ifdef FIXED_WIN_YAW_REVERSE
            motor[2] = 1000 + (2000 - rcData[YAW]);//1000~2000
#else
            motor[2] = rcData[YAW];//1000~2000
#endif
        }else{
#ifdef FIXED_WIN_YAW_REVERSE
            motor[2] = 1500 - 2000 * scaledAxisPidYaw;//rcData[YAW];//yaw 直驱 和 自动 切换
#else
            motor[2] = 1400 + 2000 * scaledAxisPidYaw;//rcData[YAW];//yaw 直驱 和 自动 切换
#endif
        }
            
        //debuginfoFastFloat[0] = scaledAxisPidRoll;//+-0.5
        //debuginfoFastFloat[1] = scaledAxisPidPitch;//+-0.5
        //debuginfoFastFloat[2] = scaledAxisPidYaw;//+-0.2

        //debuginfoFastFloat[4] = rcData[ROLL];//1000~2000
        //debuginfoFastFloat[5] = rcData[PITCH];//1000~2000
        //debuginfoFastFloat[6] = rcData[YAW];//1000~2000

        //debuginfoFastFloat[8] = motor[0];
        //debuginfoFastFloat[9] = motor[1];
        //debuginfoFastFloat[10] = motor[2];

        //debuginfoFastFloat[12] = rcCommand[ROLL];
        //debuginfoFastFloat[13] = rcCommand[PITCH];
        //debuginfoFastFloat[14] = rcCommand[YAW];

    if (!ARMING_FLAG(ARMED)) {
        motor[3] = 1000;
    } else {
        motor[3] = rcData[THROTTLE];
    }
#else
    // Disarmed mode
    if (!ARMING_FLAG(ARMED)) {
        for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
            motor[i] = motor_disarmed[i];
        }

#ifdef I_AM_REMOTE_CONTROLLER
        // 遥控器用于电调测试，或者控制摄像头上下
        uint32_t adcSample1 = adcGetChannel(ADC_CHANNEL0);//0 左侧上下   下4096
        adcSample1 = adcSample1;
        adcSample1 = adcSample1 * 1000;
        adcSample1 = adcSample1 / 4096;
        adcSample1 = adcSample1 + 1000;
        motor[0] = adcSample1;
        //debuginfoFastInt[4] = motor[0];
#else

// #ifdef OLD_FRAME
//         //测试直驱，上下左右
//         //   0      a
//         // 3   1  d   b
//         //   2      c
//         if ((rcData[PITCH]) > rxConfig()->maxcheck) {
//             motor[0] = rcData[THROTTLE];
//         }
//         if ((rcData[ ROLL]) > rxConfig()->maxcheck) {
//             motor[1] = rcData[THROTTLE];
//         }
//         if ((rcData[PITCH]) < rxConfig()->mincheck) {
//             motor[2] = rcData[THROTTLE];
//         }
//         if ((rcData[ ROLL]) < rxConfig()->mincheck) {
//             motor[3] = rcData[THROTTLE];
//         }
// #else
        //测试直驱，上下左右
        //0 1  a b
        //3 2  d c
        if (((rcData[ROLL]) < rxConfig()->mincheck) && ((rcData[PITCH]) > rxConfig()->maxcheck)) {
            motor[0] = rcData[THROTTLE];
        }
        if (((rcData[ROLL]) > rxConfig()->maxcheck) && ((rcData[PITCH]) > rxConfig()->maxcheck)) {
            motor[1] = rcData[THROTTLE];
        }
        if (((rcData[ROLL]) > rxConfig()->maxcheck) && ((rcData[PITCH]) < rxConfig()->mincheck)) {
            motor[2] = rcData[THROTTLE];
        }
        if (((rcData[ROLL]) < rxConfig()->mincheck) && ((rcData[PITCH]) < rxConfig()->mincheck)) {
            motor[3] = rcData[THROTTLE];
        }
//#endif
#endif
    }else{
        if (rcData[THROTTLE] < rxConfig()->mincheck){
            for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
                motor[i] = motor_disarmed[i];
            }            
        }
    }
#endif

#ifdef IS_BOAT
    if (ARMING_FLAG(ARMED)) {
    motor[0] = rcData[THROTTLE];
    }else{
    motor[0] = 1000;
    }
    motor[1] = rcData[ROLL];
#endif

}

float scaledAxisPidRoll;
float scaledAxisPidPitch;
float scaledAxisPidYaw;

void mixTable()
{
    float motorMix[MAX_SUPPORTED_MOTORS];

    // Find min and max throttle based on conditions. Throttle has to be known before mixing
    calculateThrottleAndCurrentMotorEndpoints();
    int err = 0;//TODO NaN时停机
    if(isnan(axisPID_P[FD_ROLL] )){err=1;}
    if(isnan(axisPID_P[FD_PITCH])){err=1;}
    if(isnan(axisPID_P[FD_YAW]  )){err=1;}

    if(isnan(axisPID_I[FD_ROLL] )){err=1;}
    if(isnan(axisPID_I[FD_PITCH])){err=1;}
    if(isnan(axisPID_I[FD_YAW]  )){err=1;}

    if(isnan(axisPID_D[FD_ROLL] )){err=1;}
    if(isnan(axisPID_D[FD_PITCH])){err=1;}
    if(isnan(axisPID_D[FD_YAW]  )){err=1;}

    if(err){
        for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
            motor[i] = 1000;
        }
        return;
    }

    // Calculate and Limit the PIDsum
    scaledAxisPidRoll  =  constrainf((axisPID_P[ FD_ROLL] + axisPID_I[ FD_ROLL] + axisPID_D[ FD_ROLL]) / PID_MIXER_SCALING,    -pidSumLimit,    pidSumLimit);
    scaledAxisPidPitch =  constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING,    -pidSumLimit,    pidSumLimit);
    scaledAxisPidYaw   = -constrainf((axisPID_P[  FD_YAW] + axisPID_I[  FD_YAW] + axisPID_D[  FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw);
    if (isMotorsReversed()) {
        scaledAxisPidRoll = -scaledAxisPidRoll;
        scaledAxisPidPitch = -scaledAxisPidPitch;
        scaledAxisPidYaw = -scaledAxisPidYaw;
    }
    if (mixerConfig()->yaw_motors_reversed) {
        scaledAxisPidYaw = -scaledAxisPidYaw;
    }

    // Calculate voltage compensation
    //const float vbatCompensationFactor = (vbatPidCompensation)  ? calculateVbatPidCompensation() : 1.0f;


    // Find roll/pitch/yaw desired output
    float motorMixMax = 0, motorMixMin = 0;
    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
        float mix =
            scaledAxisPidRoll  * currentMixer[i].roll +
            scaledAxisPidPitch * currentMixer[i].pitch +
            scaledAxisPidYaw   * currentMixer[i].yaw;

        //mix *= vbatCompensationFactor;  // Add voltage compensation

        if (mix > motorMixMax) {
            motorMixMax = mix;
        } else if (mix < motorMixMin) {
            motorMixMin = mix;
        }
        motorMix[i] = mix;
    }

    motorMixRange = motorMixMax - motorMixMin;

    if (motorMixRange > 1.0f) {
        for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
            motorMix[i] /= motorMixRange;
        }
        // Get the maximum correction by setting offset to center when airmode enabled
        if (isAirmodeActive()) {
            throttle = 0.5f;
        }
    } else {
        if (isAirmodeActive() || throttle > 0.5f) {  // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
            const float throttleLimitOffset = motorMixRange / 2.0f;
            throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
        }
    }

    // Apply the mix to motor endpoints
    applyMixToMotors(motorMix);
}
